#include <pthread.h>
#include <rclcpp/rclcpp.hpp>

// MutuallyExclusive:
// timer1触发回调函数，等待5s,期间timer2触发回调函数，但是不会被执行，而是一直等待timer1的回调函数执行完毕后，才会执行timer2的回调函数。
// Reentrant:
// timer1触发回调函数，等待5s,期间timer2触发回调函数，会两个回调函数都被执行，
class MultiThreadExecutorNode : public rclcpp::Node {
public:
  MultiThreadExecutorNode(const std::string &node_name) : Node(node_name) {
    callback_group_timer1_ = this->create_callback_group(
        rclcpp::CallbackGroupType::MutuallyExclusive);
    // callback_group_timer1_ =
    //     this->create_callback_group(rclcpp::CallbackGroupType::Reentrant);
    // create timer
    timer1_ = this->create_wall_timer(
        std::chrono::seconds(2),
        [this]() {
          RCLCPP_INFO(this->get_logger(), "Timer 1 is triggered");
          RCLCPP_INFO(this->get_logger(), "Timer 1 thread pid is: %ld",
                      pthread_self());
          RCLCPP_INFO(this->get_logger(), "Timer 1 loop 5 times:");
          int i = 5;
          while (i-- > 0) {
            rclcpp::sleep_for(std::chrono::seconds(1));
            RCLCPP_INFO(this->get_logger(), "Timer 1 loop %d", i);
          }
        },
        callback_group_timer1_);
    timer2_ = this->create_wall_timer(
        std::chrono::seconds(1),
        [this]() {
          RCLCPP_INFO(this->get_logger(), "Timer 2 is triggered");
          RCLCPP_INFO(this->get_logger(), "Timer 2 thread pid is: %ld",
                      pthread_self());
          RCLCPP_INFO(this->get_logger(), "Timer 2 loop 5 times:");
          int i = 5;
          while (i-- > 0) {
            rclcpp::sleep_for(std::chrono::seconds(1));
            RCLCPP_INFO(this->get_logger(), "Timer 2 loop %d", i);
          }
        },
        callback_group_timer1_);
  }

private:
  rclcpp::TimerBase::SharedPtr timer1_;
  rclcpp::TimerBase::SharedPtr timer2_;
  // callbcak group
  rclcpp::CallbackGroup::SharedPtr callback_group_timer1_;
};

int main(int argc, char *argv[]) {
  rclcpp::init(argc, argv);
  auto node = std::make_shared<MultiThreadExecutorNode>(
      "multi_thread_callback_group_node");
  rclcpp::executors::MultiThreadedExecutor executor;
  executor.add_node(node);
  RCLCPP_INFO(rclcpp::get_logger("thread_number"), "number of threads: %ld",
              executor.get_number_of_threads());
  executor.spin();
  rclcpp::shutdown();
  return 0;
}